// Configuracion para PWM.
/*

frecuencia de trabajo 20 Khz @ clk=80Mhz

PWM1: 
	PWM1_H RE0, PWM2_L RE1,  enable RD4,OC5.
PWM2: 
	PWM1_H RE2, PWM2_L RE3,  enable RD5,OC6.
PWM3: 
	PWM1_H RE4, PWM2_L RE5,  enable RD6,OC7.
PWM4: 
	PWM1_H RE6, PWM2_L RE7,  enable RD7,OC8.

La direccion se maneja con  PWMx_H y PWMx_L  el pwm se controla con OCx.

*/

#include "p30F6010.h"

// init_pwms  , inicializa timer3, pwm motor como I-O, o OCx como PWM.
// resolucion de PWM  0 a 1000.   20Khz.


// MACROS PARA MANEJAR DIRECCION DE MOTORES
//  no funciona en operaciones directas en PORTE, se requiere temp.


unsigned int uiMotorOffMask[]={0xfffc,0xfff3,0xffcf,0xff3f};

#define Mx_get(t)	t=PORTE	
#define Mx_off(t,x)	t&=uiMotorOffMask[x-1]
#define Mx_cw(t,x)	t|=(1<<(2*x-2))
#define Mx_ccw(t,x)	t|=(1<<(2*x-1))
#define Mx_set(t)	PORTE=t	





void vAmarantaPwmInit(void)
{
	int temp,index;
	
	TRISE=0x300;	// pwm de motores como puertos de salida
	
	TRISD=TRISD&0xff0f;		// Salidas OCx
	
	// timer 3 setup
	
	T3CON=0;  
	T3CONbits.TCS=0;   		// reloj Fosc/4.  20 MHZ
	T3CONbits.TCKPS=0;		// prescaler 1:1
	PR3=1000;				// para frecuencia = 20 KHZ , resolucion 1000 pasos + direccion
	
	// OC5 setup
	
	OC5R=0;
	OC5CON=0;			  // valor en cero.		
	OC5CONbits.OCSIDL=0;  // salida continua
	OC5CONbits.OCTSEL=1;  // fuente timer3
	OC5CONbits.OCM=6;	  // pulsos continuos activo alto
	OC5RS=0;				  // salida pwm en cero
	
	// OC6 setup
	OC6R=0;
	OC6CON=0;			  // valor en cero.		
	OC6CONbits.OCSIDL=0;  // salida continua
	OC6CONbits.OCTSEL=1;  // fuente timer3
	OC6CONbits.OCM=6;	  // pulsos continuos activo alto
	OC6RS=0;				  // salida pwm en cero
	
	// OC7 setup
	OC7R=0;
	OC7CON=0;			  // valor en cero.		
	OC7CONbits.OCSIDL=0;  // salida continua
	OC7CONbits.OCTSEL=1;  // fuente timer3
	OC7CONbits.OCM=6;	  // pulsos continuos activo alto
	OC7RS=0;				  // salida pwm en cero
	
	// OC8 setup
	OC8R=0;
	OC8CON=0;			  // valor en cero.		
	OC8CONbits.OCSIDL=0;  // salida continua
	OC8CONbits.OCTSEL=1;  // fuente timer3
	OC8CONbits.OCM=6;	  // pulsos continuos activo alto
	OC8RS=0;				  // salida pwm en cero
	
	Mx_get(temp);	/* Leer el estado de los motores */

	for (index=1;index<=4;index++)	/* Apagar los motores */
		Mx_off(temp,index);

	/* Ejecutar la orden de los motores */
	Mx_set(temp);
		
		
								
	T3CONbits.TON=1;	  // timer ON	
	
}

void vAmarantaPwmSet(char pwmId, int value) // salida a pwm , N = # de PWM, V= valor, entero con signo
{

	int temp;
	
	Mx_get(temp);		/* Obtener el estado del motor */
	Mx_off(temp,pwmId)	/* Compilar orden de apagado del motor pwmId */
	
	/* Dependiendo si la orden es negativa o positiva, calcular la orden CCW/CW y el valor corrector de pwm */
	if (value & 0x8000){
		
		/* Valor del PWM */
		value = ~value+1;
		/* Orden CCW */
		Mx_ccw(temp,pwmId);
	
	}
	
	else
		
		/* Orden CW */
		Mx_cw(temp,pwmId);
	
	/* Ejecutar la orden de PWM */
	switch (pwmId) {
	
	
		case 1 : 
			OC5RS=value;	// salida pwm
		break;
	
		case 2 : 
			OC6RS=value;	// salida pwm 
		break;
	
		case 3 : 
			OC7RS=value;	// salida pwm 
		break;
	
		case 4 : 
			OC8RS=value;	// salida pwm 
		break;
	
	}
	/* Ejecutar la orden en los motores */
	Mx_set(temp);
	
	
}
